function X = TDCP(ph1_R,satellite_position,positionreceiver)
% Calculate the delta position of vehicle in ECEF between two epochs  
% from carrier phase when no cycle slip
% Inputs: ph1_R:carrier phase vector
%         satellite_position:position of satellite in ECEF
%         positionreceiver:approximate position of receiver in ECEF
% Output: X=[deltaX deltaY deltaZ delta_clock_of_receiver] in ECEF in m 
%    [deltaX deltaY deltaZ] is the change of vehicle in position between two epochs
%     delta_clock_of_receiver is receiver clock error by difference two epochs
%----------------------------------------------------------------------------------------------
%                           iTAG_VAD v1.0
%
% Copyright (C) Rui Sun, Qi Cheng and Junhui Wang(2020)
%
% 
%----------------------------------------------------------------------------------------------

GPSEPOCHS=length(ph1_R);
c=299792458;  %speed of light in m/s
f1=1575420000; %L1 frequency in Hz 
f2=1227600000; %L2 frequency in Hz 
LAMBDA1=c/f1;  %l1 wavelength in m
LAMBDA2=c/f2;  %l1 wavelength in m
X=[];       
step=1;
m=0;
for i=1+step:step:GPSEPOCHS

    satnum1=find(ph1_R(:,i-1));
    satnum2=find(ph1_R(:,i));
    satnum3=intersect(satnum1,satnum2);
    satnum4=find(satellite_position(:,1,i-1));
    satnum5=find(satellite_position(:,1,i));
    satnum6=intersect(satnum4,satnum5);
    satnum=intersect(satnum3,satnum6);
    if length(satnum)<4
        x=[NaN;NaN;NaN;NaN];
        X=[X x];
        m=0;
        continue
    end
    B=[];
    C=[];
    UNIT=[];
    
    for j=1:length(satnum)
        VECTOR=squeeze(satellite_position(satnum(j),:,i))-positionreceiver(i,:);
        unit=VECTOR/norm(VECTOR);
        UNIT=[UNIT;unit];
        B=[B;
            unit(1) unit(2) unit(3) -1];
        deltaS=satellite_position(satnum(j),:,i)-satellite_position(satnum(j),:,i-1);
        deltaPHASE=ph1_R(satnum(j),i)-ph1_R(satnum(j),i-1);
        C=[C;dot(unit,deltaS)-LAMBDA1*deltaPHASE];
    end
    %   calculate delta x from carrier phase
    x=inv(B'*B)*B'*C;
    if (norm(x(1:3))<0.2)&(m>0)   % If x is smaller than 20 cm, use the last x
        x=m;
    end
    m=x;
    X=[X x];
end
    